// --------------------------------------------------------------------------------------------------------------------
// <copyright file="LaserHorizontalDepthProfile.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Microsoft.Robotics.Navigation.Perception
{
    using System;
    using System.Collections.Generic;
    using System.Linq;
    using System.Runtime.CompilerServices;
    using System.Runtime.Serialization;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Vision;
    using Microsoft.Robotics.Vision.Cameras;

    /// <summary>
    /// Builds a horizontal depth profile from a LIDAR scan line
    /// </summary>
    [DataContract]
    public class LaserHorizontalDepthProfile : IHorizontalDepthProfile
    {
        /// <summary>
        /// The default resolution of a Laser scan. 
        /// </summary>
        private const int DefaultReadingsPerLine = 180;

        /// <summary>
        /// Maximum laser range.
        /// </summary>
        private const double MaximumRangeInMeters = 40;

        /// <summary>
        /// The depth values (measured along the ray).  Depth values are ordered left to right, just like the Kinect image.
        /// </summary>
        [DataMember]
        private double[] depthValues;

        /// <summary>
        /// The cached set of occupied points. 
        /// </summary>
        [DataMember]
        private Point2D[] points;

        /// <summary>
        /// The number of readings in the Laser scan
        /// </summary>
        [DataMember]
        private int readingsPerScanLine;

        /// <summary>
        /// The angular resolution of the scan line, in radians per pixel
        /// </summary>
        [DataMember]
        private double angularResolution;

        /// <summary>
        /// Initializes a new instance of the LaserHorizontalDepthProfile class.
        /// </summary>
        /// <param name="pose">Pose of the sensor in the horizontal plane</param>
        /// <param name="readingsPerScanLine">The resolution of the Laser scan</param>
        /// <param name="angularResolution">The angular resolution of the scan line</param>
        public LaserHorizontalDepthProfile(Pose2D pose, int readingsPerScanLine = DefaultReadingsPerLine, double angularResolution = MathConstants.Degrees2Radians)
        {
            this.readingsPerScanLine = readingsPerScanLine;
            this.angularResolution = angularResolution;
            this.points = new Point2D[readingsPerScanLine];
            this.depthValues = new double[readingsPerScanLine];
            this.Pose = pose;
        }

        /// <summary>
        /// Initializes a new instance of the LaserHorizontalDepthProfile class.
        /// </summary>
        /// <param name="scan">The scan line to initialize from</param>
        /// <param name="pose">Pose of the Laser in the horizontal plane</param>
        public LaserHorizontalDepthProfile(double[] scan, Pose2D pose)
            : this(pose, scan.Length)
        {
            int halfWidth = this.readingsPerScanLine / 2;

            this.depthValues = scan;
            for (int i = 0; i < this.readingsPerScanLine; i++)
            {
                if (this.depthValues[i] > MaximumRangeInMeters)
                {
                    this.depthValues[i] = 0;
                }

                double lateralDisplacement = this.depthValues[i] * Math.Sin((halfWidth - i) * this.angularResolution);
                double distanceFromPlane = this.depthValues[i] * Math.Cos((halfWidth - i) * this.angularResolution);
                this.points[i] = new Point2D(distanceFromPlane, lateralDisplacement);
            }
        }

        /// <summary>
        /// Gets the number of entries (bins) in the horizontal depth profile.
        /// </summary>
        public int Length
        {
            get
            {
                return this.readingsPerScanLine;
            }
        }

        /// <summary>
        /// Gets the pose of the center of the horizontal depth profile.  
        /// </summary>
        [DataMember]
        public Pose2D Pose
        {
            get;
            private set;
        }

        /// <summary>
        /// Gets the angular field of view of the depth profile
        /// </summary>
        public double FOV
        {
            get { return this.angularResolution * this.readingsPerScanLine; }
        }

        /// <summary>
        /// Gets the maximum distance in the scan line
        /// </summary>
        public double MaximumDistance
        {
            get { return this.depthValues.Max(); }
        }

        /// <summary>
        /// Returns the maximum width of the field of view given a depth cutoff distance. For a 180 or 360 laser, this is the diameter of the field of view.
        /// </summary>
        /// <param name="maxDepth">The maximum depth distance</param>
        /// <returns>The width of the field of view at the given depth</returns>
        public double GetFOVWidthForDepth(double maxDepth)
        {
            return 2 * maxDepth * Math.Sin(this.angularResolution * this.readingsPerScanLine / 2);
        }

        /// <summary>
        /// Returns the angle of the ray corresponding to the given index.
        /// </summary>
        /// <param name="index">The index of a depth value of hit points</param>
        /// <returns>The corresponding angle.</returns>
        public double GetAngleForIndex(int index)
        {
            return (index - this.readingsPerScanLine / 2) * this.angularResolution;
        }

        /// <summary>
        /// Returns the distance to the closest obstacle along the specified ray (distance is measured to the focal point, i.e. along the ray). Depth values are ordered left to right, just like the Kinect image.
        /// </summary>
        /// <param name="index">Index of the ray / pixel</param>
        /// <returns>The distance to the obstacle along the given ray / pixel</returns>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public double GetHitDistance(int index)
        {
            return this.depthValues[index];
        }

        /// <summary>
        /// Returns the closest occupied point in the sensor coordinate frame for the given ray / pixel index
        /// </summary>
        /// <param name="index">Index of the ray / pixel</param>
        /// <returns>The coordinates of the closest occupied point in the sensor coordinate frame</returns>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public Point2D GetHitPoint(int index)
        {
            return this.points[index];
        }

        /// <summary>
        /// Adds an observed obstacle specified in local coordinates.
        /// Has no effect if the profile already contains a smaller depth value at that heading.
        /// </summary>
        /// <param name="observation">The observation to add, in local sensor coordinates (x represents depth, along the optical axis, y is positive to the left of the optical axis).</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void AddHitPoint(Point2D observation)
        {
            double distance;
            int index = this.Reproject(observation, out distance);
            if (index < 0 || index >= this.readingsPerScanLine)
            {
                return;
            }

            if (this.points[index].X == 0 || observation.X < this.points[index].X)
            {
                this.depthValues[index] = distance;
                this.points[index] = observation;
            }
        }

        /// <summary>
        /// Given a point, it returns the column it would project into.
        /// </summary>
        /// <param name="observation">The point in local coordinates</param>
        /// <param name="distance">The distance to the specified point along the ray</param>
        /// <returns>The index of the column the point would project to</returns>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public int Reproject(Point2D observation, out double distance)
        {
            int index = (int)(this.readingsPerScanLine / 2 - Math.Atan2(observation.Y, observation.X) / this.angularResolution + 0.5);
            if (index < 0 || index >= this.readingsPerScanLine)
            {
                distance = 0;
                return -1;
            }

            distance = Math.Sqrt(observation.X * observation.X + observation.Y * observation.Y);
            return index;
        }

        /// <summary>
        /// Creates an empty horizontal depth profile with the same structure as this one.
        /// </summary>
        /// <returns>An empty depth profile</returns>
        public IHorizontalDepthProfile Create()
        {
            return new LaserHorizontalDepthProfile(this.Pose, this.readingsPerScanLine, this.angularResolution);
        }
    }
}
